function [ParetoFront, ParetoSet, GenerationNum, EvaluationNum, OptSetRemain] = nsga2_multimodal_test(Problem, Algorithm, OptSet)
%NSGA-II: real coded ga, binary tournament selection, Simulated Binary
%   Crossover (SBX) operator, polynomial mutation, seeking for weak pareto,
%   clearing is used in the non-dominated sorting
%The decision variable has d dimensions and there are m objective functions
%INPUT
%	Problem.
%       Dimension: scalar, the dimension of the decision variables
%       LowerBound: 1 -by- d vector, the lower bound of the decision variables
%       UpperBound: 1 -by- d vector, the upper bound of the decision variables
%       NumObjFunc: scalar, number of objective function
%       ObjFunc: the function handle of the objective funtions,
%           y = ObjFunc(x); x: n -by- d matrix; y: n -by- m matrix.
%   Algorithm.
%       PopulationSize: scalar, the population size
%       StopCriteria: [X,Y]
%           [1,Y]: reach maximum budget Y
%           [2,Y]: reach the maximal generation number Y
%       (the followings are not necessary)
%       pc: crossover probability, default value: 0.9
%       pm: mutation probability, default value: 1/d
%       eta_c: SBX index, default value: 10
%       eta_m: mutation index, default value: 20
%       ParetoType: default value: 1
%           1: normal pareto;
%           [2, delta_f, delta_x]: weak pareto
%   OptSet.
%       sol: ~ -by- d matrix, the optimal solution
%       fval: scalar, the global optimum
%       epsilon: scalar, the difference of the value considered to be optimum
%       radius: scalar, the distance of the solution considered to be found
%OUTPUT
%   ParetoFront: the pareto frontier
%   ParetoSet: the solutions in the pareto frontier
%   GenerationNum: the total number of generations
%   EvaluationNum: the total number of evaluations
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan. A Fast
%   Elitist Multi-objective Genetic Algorithm: NSGA-II. IEEE Transactions
%   on Evolutionary Computation, 6(2):182-197, April 2002.
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% contact linziwei@sjtu.edu.cn
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% PROBLEM ASSIGNMENT
d = Problem.Dimension;
LB = Problem.LowerBound;
UB = Problem.UpperBound;
NumObj = Problem.NumObjFunc;
ObjFunc = Problem.ObjFunc;
% ALGORITHM ASSIGNMENT
PopulationSize = Algorithm.PopulationSize;
StopCriteria = Algorithm.StopCriteria;
if isfield(Algorithm,'pc')
    pc = Algorithm.pc;
else
    pc = 0.9;
end
if isfield(Algorithm,'pm')
    pm = Algorithm.pm;
else
    pm = 1/d;
end
if isfield(Algorithm,'eta_c')
    eta_c = Algorithm.eta_c;
else
    eta_c = 10;
end
if isfield(Algorithm,'eta_m')
    eta_m = Algorithm.eta_m;
else
    eta_m = 20;
end
if isfield(Algorithm,'ParetoType')
    ParetoType = Algorithm.ParetoType;
else
    ParetoType = 1;
end
N = (StopCriteria(1)==1)*StopCriteria(2) + (StopCriteria(1)~=1)*1000000; % the maximal budget size
% the optimal set for testing purpose
if ~isempty(OptSet)
    OptSetRemain = OptSet.sol;
end
%% INITIALIZATION
GenerationNum = 1;  % the current generation number
population = rand([PopulationSize,d]).*repmat((UB-LB),[PopulationSize,1])+repmat(LB,[PopulationSize,1]);
fitness = ObjFunc(population);
population = [zeros(PopulationSize,2),fitness,population];  % [non-dominated rank, crowded rank, fitnesses, solution]
EvaluationNum = PopulationSize;  % the budget used
% remove the found optimum
if ~isempty(OptSet)
    remove_opt( [population(:,3), population(:,(end-d+1):end)] )
end
maxobj = max(fitness);
minobj = min(fitness);
population = non_dominated_sort(population);
population = crowded_comparison(population);
%% EVOLUTION
population_merge = zeros(PopulationSize*2,2+NumObj+d);
while EvaluationNum<N && (StopCriteria(1,1)~=2 || GenerationNum<StopCriteria(1,2))...
        && (isempty(OptSet) || ~isempty(OptSetRemain))
    population_merge(1:PopulationSize,:) = population;
    population_merge((PopulationSize+1):end,:) = 0;
    % New Generation
    GenerationNum = GenerationNum+1;
    NumNew = 1;
    for i = 1:round((PopulationSize/2))
        % selection
        iparents = selection_tournament(population(:,2));
        if rand()<pc
            % crossover
            children = crossover_sbx(population(iparents,(3+NumObj):end));
            if rand()<pm
                % mutation
                children = mutation_polynomial(children);
            end
        elseif rand()<pm
            % mutation
            children = mutation_polynomial(population(iparents,(3+NumObj):end));
        else
            continue
        end
        if NumNew < PopulationSize
            population_merge(PopulationSize+[NumNew,NumNew+1],(3+NumObj):end) = children;
            NumNew = NumNew+2;
        else
            population_merge(PopulationSize*2,(3+NumObj):end) = children(1,:);
            NumNew = NumNew+1;
        end
    end
    NumNew = NumNew-1;
    NewId = (PopulationSize+1):(PopulationSize+NumNew);
    population_merge(NewId,3:(2+NumObj)) = ObjFunc(population_merge(NewId,(3+NumObj):end));
    EvaluationNum = EvaluationNum+NumNew;
    % remove the found optimum
    if ~isempty(OptSet)
        remove_opt( population_merge(NewId,3:end) )
    end
    % sort all solution according to the non-dominated ranking
    mergeId = 1:(PopulationSize+NumNew);
    population_merge(mergeId,:) = non_dominated_sort(population_merge(mergeId,:));
    population_merge(mergeId,:) = crowded_comparison(population_merge(mergeId,:));
    [~, solutionId] = sort(population_merge(mergeId,2));
    population_merge(mergeId,:) = population_merge(solutionId,:);
    % the individuals to survive
    population = population_merge(1:PopulationSize,:);
    population = crowded_comparison(population);
end

ParetoFront = population(population(:,1)==1,3:(2+NumObj));
ParetoSet = population(population(:,1)==1,(3+NumObj):end);

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
    function [dominate_or_not] = dominate(individual1, individual2)
    % dominate_or_not = 1: individual1 dominate individual2
    %                   0: individual1 does not dominate individual2
        if ParetoType(1)==1
            % strong pareto
            dominate_or_not = (sum(individual1(1:NumObj)<=individual2(1:NumObj))==NumObj)...
                *(sum(individual1(1:NumObj)<individual2(1:NumObj))>0);
        else
            delta_f = ParetoType(2);
            delta_x = ParetoType(3);
            dominate_or_not = 1;
            % weak pareto
            if (individual1(2)>=individual2(2)) || (individual1(1)>individual2(1))
                dominate_or_not = 0;
            elseif (individual2(1)-individual1(1))<delta_f
                if sqrt(sum((individual1((NumObj+1):end)-individual2((NumObj+1):end)).^2))>delta_x
                    dominate_or_not = 0;
                end
            end
%             dominate_or_not = (sum(individual1(1:NumObj)<individual2(1:NumObj))==NumObj);
        end
    end

    function [population] = non_dominated_sort(population)
    % Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan. A Fast
    %   Elitist Multi-objective Genetic Algorithm: NSGA-II. IEEE Transactions
    %   on Evolutionary Computation, 6(2):182-197, April 2002.
    %population: [non-dominated rank, crowded rank, fitnesses, solution]
        population(:,1:2) = 0;
        population_size_f = size(population,1);
        SS = cell(population_size_f,1);  % the solutions that be dominated by p
        nn = zeros(population_size_f,1);  % the number of solutions that dominate p
        Front_f = cell(population_size_f,1);  % the non-dominated front
        for p = 1:population_size_f
            for q = 1:population_size_f
                if dominate(population(p,3:(2+NumObj)), population(q,3:(2+NumObj)))
                    SS{p} = [SS{p},q];
                elseif dominate(population(q,3:(2+NumObj)), population(p,3:(2+NumObj)))
                    nn(p) = nn(p)+1;
                end
            end
            if nn(p) == 0
                population(p,1) = 1;
                Front_f{1} = [Front_f{1},p];
            end
        end
        ii = 1;
        while ~isempty(Front_f{ii})
            next_front = [];
            for p = Front_f{ii}
                for q = SS{p}
                    nn(q) = nn(q)-1;
                    if nn(q)==0
                        population(q,1) = ii+1;
                        next_front = [next_front,q];
                    end
                end
            end
            ii = ii+1;
            Front_f{ii} = next_front;
        end
    end

    function [population] = crowded_comparison(population)
    % Kalyanmoy Deb, Amrit Pratap, Sameer Agarwal, and T. Meyarivan. A Fast
    %   Elitist Multi-objective Genetic Algorithm: NSGA-II. IEEE Transactions
    %   on Evolutionary Computation, 6(2):182-197, April 2002.
    %population: [non-dominated rank, crowded rank, fitnesses, solution]
        max_front = max(population(:,1));
        rank_finished = 0;
        for ii = 1:max_front
            solutionId_f = find(population(:,1)==ii);
            fitness_temp = population(solutionId_f,3:(2+NumObj));
            solutionN_f = length(solutionId_f);
            distance = zeros(solutionN_f,1);
            for jj = 1:NumObj
                [~,rank_f] = sort(fitness_temp(:,jj));
                distance(rank_f(1)) = inf;
                distance(rank_f(end)) = inf;
                for kk = 2:(solutionN_f-1)
                    distance(rank_f(kk)) = distance(rank_f(kk))...
                        +(fitness_temp(rank_f(kk+1),jj)-fitness_temp(rank_f(kk-1),jj))/(maxobj(jj)-minobj(jj));
                end
            end
            [~,rank_f] = sort(distance,'descend');
            population(solutionId_f(rank_f),2) = (1:solutionN_f)'+rank_finished;
            rank_finished = rank_finished+solutionN_f;
        end
    end

    function [iparents] = selection_tournament(ranks)
        iparent1 = ceil(rand([1,2]).*PopulationSize);
        [~, bestid] = min(ranks(iparent1));
        iparent1 = iparent1(bestid);
        while 1
            iparent2 = ceil(rand([1,2]).*PopulationSize);
            [~, bestid] = min(ranks(iparent2));
            iparent2 = iparent2(bestid);
            if iparent2 ~= iparent1
                break
            end
        end
        iparents = [iparent1, iparent2];
    end

    function [children] = crossover_sbx(parents)
    % Kalyanmoy Deb and R. B. Agarwal. Simulated Binary Crossover for Continuous
    %   Search Space. Complex Systems, 9:115-148, April 1995.
    % eta: distribution index for crossover, high eta means with higher
    %   probability the children is very like the parents.
        uu = rand(1,d);
        beta = (2.*uu).^(1/(1+eta_c)).*(uu<0.5)...
            +1./((2.*(1-uu)).^(1/(1+eta_c))).*(uu>=0.5);
        child1 = 0.5.*(1-beta).*parents(1,:)+(1+beta).*parents(2,:);
        child2 = 0.5.*(1+beta).*parents(1,:)+(1-beta).*parents(2,:);
        child1 = max([LB;child1]);
        child1 = min([UB;child1]);
        child2 = max([LB;child2]);
        child2 = min([UB;child2]);
        children = [child1;child2];
    end

    function [children] = mutation_polynomial(parents)
        n_f = size(parents,1);
        children = zeros(n_f,d);
        for ii = 1:n_f
            uu = rand(1,d);
            delta = ((2.*uu).^(1/(eta_m+1))-1).*(uu<0.5)...
                +(1-(2.*(1-uu)).^(1/(eta_m+1))).*(uu>=0.5);
            child = parents(ii,:)+(UB-LB).*delta;
            child = max([LB;child]);
            child = min([UB;child]);
            children(ii,:) = child;
        end
    end

    function remove_opt( new_samples )
    % remove_opt: remove the found optimal solution from OptSetRemain
        new_samples(new_samples(:,1)>=(OptSet.fval+OptSet.epsilon),:) = [];
        for ii = 1:size(new_samples,1)
            OptDis = sqrt(sum((repmat(new_samples(ii,2:end),[size(OptSetRemain,1),1]) - OptSetRemain).^2,2));
            [MinDis, OptId] = min(OptDis);
            if MinDis<OptSet.radius
                OptSetRemain(OptId,:) = [];
            end
        end
    end
end

